import numpy as np

class BicycleModel:
    def __init__(self, wheelbase=2.8, max_steer=0.5236, max_accel=3.0, max_decel=-8.0):
        # 车辆参数
        self.wheelbase = wheelbase  # 轴距(m)
        self.max_steer = max_steer  # 最大转向角(rad)
        self.max_accel = max_accel  # 最大加速度(m/s²)
        self.max_decel = max_decel  # 最大减速度(m/s²)

    def update(self, state, control, dt):
        """
        车辆状态更新
        state: [x, y, theta, v] 车辆状态
        control: [delta, a] 控制输入
        dt: 时间步长(s)
        return: 新的车辆状态
        """
        x, y, theta, v = state
        delta, a = control

        # 限制控制输入
        delta = np.clip(delta, -self.max_steer, self.max_steer)
        a = np.clip(a, self.max_decel, self.max_accel)

        # 自行车模型状态更新
        dx = v * np.cos(theta)
        dy = v * np.sin(theta)
        d_theta = v / self.wheelbase * np.tan(delta)
        dv = a

        # 欧拉积分更新状态
        x_new = x + dx * dt
        y_new = y + dy * dt
        theta_new = theta + d_theta * dt
        v_new = v + dv * dt

        # 确保航向角在[-pi, pi]范围内
        theta_new = (theta_new + np.pi) % (2 * np.pi) - np.pi

        return np.array([x_new, y_new, theta_new, v_new])

    def get_state_size(self):
        return 4

    def get_control_size(self):
        return 2